#include "Esp32McpwmMotor.h"

/*
MOTORID PCNTID(ID/3+ID)      IOA((ID%3)*2)     IOB(ID*2+1)    TIMER
0       MCPWM_UNIT_0          0                1              0
1       MCPWM_UNIT_0          2                3
2       MCPWM_UNIT_0          4                5
3       MCPWM_UNIT_1          0                1
4       MCPWM_UNIT_1          2                3
5       MCPWM_UNIT_1          4                5
*/
void Esp32McpwmMotor::attachMotor(uint8_t id, uint8_t gpioIn1, uint8_t gpioIn2)
{
    // 指定使用的UNIT单元，这里用的是UNIT_0，但是这里是初始化
    mcpwm_unit_t mcpwm_num = MCPWM_UNIT_0;

    //分别得到A通道信号和B通道信号
    mcpwm_io_signals_t io_signal_a = mcpwm_io_signals_t((id % 3) * 2);
    mcpwm_io_signals_t io_signal_b = mcpwm_io_signals_t((id % 3) * 2 + 1);
    
    //当id为0-2时,使用MCPWM_UNIT_0
    //当id为3-5时,使用MCPWM_UNIT_1
    if (id / 3 == 1)
    {
        mcpwm_num = MCPWM_UNIT_1;
    }

    //初始化要使用的定时器
    mcpwm_timer_t mcpwm_timer = MCPWM_TIMER_0;

    //使用定时器1
    if (id % 3 == 1)
    {
        mcpwm_timer = MCPWM_TIMER_1;
    }
    //使用定时器2
    else if (id % 3 == 2)
    {
        mcpwm_timer = MCPWM_TIMER_2;
    }

    //指定了使用的MCPWM单元(0或1),找到他的A输出通道,设置为gpioIn1
    mcpwm_gpio_init(mcpwm_num, io_signal_a, gpioIn1);
    //指定了使用的MCPWM单元(0或1),找到他的B输出通道,设置为gpioIn2
    mcpwm_gpio_init(mcpwm_num, io_signal_b, gpioIn2);
    //管理电机的状态
    this->mMotorAttached[id] = true;

    //定义 PWM 配置的容器，包含频率、占空比、计数模式等关键参数
    mcpwm_config_t pwm_config;
    //频率设置，单位是hz
    pwm_config.frequency = 5000; 
    //初始化 A/B 通道的占空比为 0%（电机停止）
    pwm_config.cmpr_a = 0;      
    pwm_config.cmpr_b = 0;     

    //电机的驱动
    pwm_config.counter_mode = MCPWM_UP_COUNTER;
    pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
    //初始化
    mcpwm_init(mcpwm_num, mcpwm_timer, &pwm_config);
}

//电机停止
void Esp32McpwmMotor::stopMotor(int8_t motorId)
{
}

//更新电机的速度
void Esp32McpwmMotor::updateMotorSpeed(int8_t id, int16_t pwmValue)
{
    //查看使用那个控制单元
    mcpwm_unit_t mcpwm_num = MCPWM_UNIT_0;
    if (id / 3 == 1)
    {
        mcpwm_num = MCPWM_UNIT_1;
    }

    //查看使用哪个定时器
    mcpwm_timer_t mcpwm_timer = MCPWM_TIMER_0;
    if (id % 3 == 1)
    {
        mcpwm_timer = MCPWM_TIMER_1;
    }
    else if (id % 3 == 2)
    {
        mcpwm_timer = MCPWM_TIMER_2;
    }
    
    if (pwmValue > 0)
    {
        //将MCPWM操作器B设置为持续低电平
        mcpwm_set_signal_low(mcpwm_num, mcpwm_timer, MCPWM_OPR_B);     
        //将MCPWM操作器B的占空比进行设置
        mcpwm_set_duty(mcpwm_num, mcpwm_timer, MCPWM_OPR_A, pwmValue);
        //设置占空比生效模式，即高电平有效
        mcpwm_set_duty_type(mcpwm_num, mcpwm_timer, MCPWM_OPR_A, MCPWM_DUTY_MODE_0); 
    }

    else
    {
        //将MCPWM操作器A设置为持续低电平
        mcpwm_set_signal_low(mcpwm_num, mcpwm_timer, MCPWM_OPR_A);  
        //将MCPWM操作器B的占空比进行设置                
        mcpwm_set_duty(mcpwm_num, mcpwm_timer, MCPWM_OPR_B, -pwmValue);  
        //设置占空比生效模式，即高电平有效 
        mcpwm_set_duty_type(mcpwm_num, mcpwm_timer, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); 
    }
}